
import isaaclab.sim as sim_utils
from isaaclab.assets.articulation import ArticulationCfg
from isaaclab.actuators import DCMotorCfg
from arm_lab.assets.config import ISAACLAB_ASSETS_DATA_DIR

##
# Configuration
##

MICRO_UR_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/micro_ur/micro_ur.usd",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            max_depenetration_velocity=5.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0
        ),
        # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.021),
        joint_pos={
            "joint_1": 0.0,
            "joint_2": 1.571,
            "joint_3": -2.356,
            "joint_4": -0.785,
            "joint_5": 1.571,
            "joint_6": 0.0,
        },
    ),
    actuators={
        "big_joints": DCMotorCfg(
            joint_names_expr=["joint_1", "joint_2"],
            effort_limit=16.0,
            saturation_effort=16.0,
            velocity_limit=4.18,
            stiffness=80.0,     # 刚度约是阻尼的 20 倍
            damping=4.0,
            friction=0.0,
        ),
        "small_joints": DCMotorCfg(
            joint_names_expr=["joint_3", "joint_4", "joint_5", "joint_6"],
            effort_limit=6.0,
            saturation_effort=6.0,
            velocity_limit=6.28,
            stiffness=80.0,     # 刚度约是阻尼的 20 倍
            damping=4.0,
            friction=0.0,
        ) 
    },
    soft_joint_pos_limit_factor=1.0,
)